/* -*- c++ -*- */
/*
 *   Copyright 2011,2012 Erik Persson
 *
 *   This file is part of the cell-sync-usrp project.
 *
 *   cell-sync-usrp is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   cell-sync-usrp is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with cell-sync-usrp.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 * config.h is generated by configure.  It contains the results
 * of probing for features, options etc.  It should be the first
 * file included in your .cc file.
 */
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <wcdma_slot_sync_cvc.h>
#include <gr_io_signature.h>

#include <iostream>
#include <cstdio>
#include <math.h>

#include "sequences/wcdma_codes.h"


// enable to disables slot synchronization when in in sync state
//#define DISABLE_SLOT_SYNC_WHEN_IN_SYNCH

/*
 * some definitions of constants
 */



//#define DISABLE_SLOT_SYNC_WHEN_IN_SYNCH

// number of samples in a slot
#define SAMPLES_PER_SLOT 5120
#define SAMPLES_PER_FRAME (SAMPLES_PER_SLOT*15)

// number of slots to wait for before setting the slot hit index
#define WRAP_COUNTER_RESET 15*3-1

// number of samples on STATE_WAIT_ON_RADIO to wait for
#define WAIT_ON_RADIO_NOF_SAMPLES (SAMPLES_PER_FRAME)

// the states of the block
#define STATE_WAIT_ON_RADIO 0
#define STATE_FIND_SLOT_SYNC 1
#define STATE_FIND_FRAME_SYNC 2
#define STATE_IN_SYNC 3

#define MAX_STATE_INDEX 10
#define STATE_DATA_SIZE 1024*200*4

/*
 * Create a new instance of wcdma_slot_sync_cvc and return
 * a boost shared_ptr.  This is effectively the public constructor.
 */
wcdma_slot_sync_cvc_sptr
wcdma_make_slot_sync_cvc ()
{
  return wcdma_slot_sync_cvc_sptr (new wcdma_slot_sync_cvc ());
}

/*
 * Specify constraints on number of input and output streams.
 * This info is used to construct the input and output signatures
 * (2nd & 3rd args to gr_block's constructor).  The input and
 * output signatures are used by the runtime system to
 * check that a valid number and type of inputs and outputs
 * are connected to this block.  In this case, we accept
 * only 1 input and 1 output.
 */
static const int MIN_IN = 1;	// mininum number of input streams
static const int MAX_IN = 1;	// maximum number of input streams
static const int MIN_OUT = 1;	// minimum number of output streams
static const int MAX_OUT = 1;	// maximum number of output streams



/*
 * The private constructor
 */
wcdma_slot_sync_cvc::wcdma_slot_sync_cvc ()
  : gr_block ("slot_sync_cvc",
		   gr_make_io_signature (MIN_IN, MAX_IN, sizeof (gr_complex)),
		   gr_make_io_signature (MIN_OUT, MAX_OUT, sizeof (gr_complex)))
{

  this->d_done = 0;

  this->d_corr_buff_pos = 0;	// clear the index into the d_corr_buffer
  this->d_acc_buff_pos = 0;		// clear the index into the d_acc_buffer

  this->d_acc_buff_hit_index = -1;	// clear the hit index (-1 no hit)
  this->d_acc_buff_wrap_cntr = 0;	// clear the wrap counter

  this->d_state_data = new unsigned char[STATE_DATA_SIZE];	// allocate state data

  this->setState(STATE_WAIT_ON_RADIO,NULL);		// set initial state
//  find_frame_sync_state_data *tmp = (find_frame_sync_state_data*) this->d_state_data;
//  tmp->scrambling_group=12;
//  this->setState(STATE_IN_SYNC,NULL);

  //this->setState(STATE_FIND_SLOT_SYNC,NULL);		// for debug purposes

  // set up methods for handling the different states
  this->d_state_functions = new state_function_t[MAX_STATE_INDEX]();
  this->d_state_functions[STATE_WAIT_ON_RADIO] = &wcdma_slot_sync_cvc::state_wait_on_radio;
  this->d_state_functions[STATE_FIND_SLOT_SYNC] = &wcdma_slot_sync_cvc::state_find_slot_sync;
  this->d_state_functions[STATE_FIND_FRAME_SYNC] = &wcdma_slot_sync_cvc::state_find_frame_sync;
  this->d_state_functions[STATE_IN_SYNC] = &wcdma_slot_sync_cvc::state_in_sync;

  // create the empty(filled with zeroes) d_corr_buffer
  this->d_corr_buffer = new std::vector<gr_complex> (SAMPLES_PER_SLOT/10);

  // create the empty(filled with zeroes) d_acc_buffer
  this->d_acc_buffer = new std::vector<float> (SAMPLES_PER_SLOT);

  this->d_PCCPCH_frame_buffer = new std::vector<unsigned char> (PCCPCH_FRAME_BUFFER_SIZE*270);

  this->d_number_of_retries = 0;
}


/*
 * Our virtual destructor.
 */
wcdma_slot_sync_cvc::~wcdma_slot_sync_cvc ()
{
	delete this->d_state_functions;
	delete this->d_corr_buffer;
	delete this->d_acc_buffer;
	delete this->d_PCCPCH_frame_buffer;
}


int wcdma_slot_sync_cvc::general_work (int noutput_items,
		  gr_vector_int &ninput_items,
		  gr_vector_const_void_star &input_items,
		  gr_vector_void_star &output_items)
{
  // calling the function handling the current state
  (this->*this->d_state_functions[this->d_state]) (noutput_items,ninput_items,input_items,output_items);
}


/*
 * Performs correlation and returns the result
 */
float wcdma_slot_sync_cvc::correlate(int code_number)
{
	// initialize variables
	gr_complex sum=0;
	int index = this->d_corr_buff_pos+1;
	int index2 = 0;

	int codelen = 512;

	if (index>=(*this->d_corr_buffer).size()){
		index = 0;
	}

	// correlate first position
	sum += (*this->d_corr_buffer)[index] * sync_codes[code_number][index2];
	index++;
	index2++;
	if (index>=(*this->d_corr_buffer).size()){
		index = 0;
	}
	// loop through and perform the rest of the correlation
	while (index != this->d_corr_buff_pos){
		sum += (*this->d_corr_buffer)[index] * sync_codes[code_number][index2];

		index++;
		index2++;
		if (index>=(*this->d_corr_buffer).size()){
			index = 0;
		}
//		if (index2>codelen-1) {
//			break;
//		}
	}

	gr_complex a = sum;// * sum;
	float powlen = a.real()*a.real() + a.imag()*a.imag();
	return powlen;

	//float alen = sqrt(powlen);
	//return alen*alen;

	//return sum;//*sum;
}


float wcdma_slot_sync_cvc::correlate(gr_complex *buffer, int code_number)
{
	// initialize variables
		gr_complex sum=0;
		int index = 0;

		for (int i=0;i<512;i++){
			sum += buffer[i] * sync_codes[code_number][i];
		}

		gr_complex a = sum;// * sum;
		float powlen = a.real()*a.real() + a.imag()*a.imag();
		return powlen;
}

void wcdma_slot_sync_cvc::copy_last_512_samples(gr_complex *buffer)
{
	// initialize variables
	gr_complex sum=0;
	int index = this->d_corr_buff_pos+1;
	int index2 = 0;

	if (index>=(*this->d_corr_buffer).size()){
			index = 0;
		}

	// copy first position
	buffer[index2] = (*this->d_corr_buffer)[index];
	index++;
	index2++;
	if (index>=(*this->d_corr_buffer).size()){
		index = 0;
	}
	// loop through and perform the rest of the copying
	while (index != this->d_corr_buff_pos){
		buffer[index2] = (*this->d_corr_buffer)[index];

		index++;
		index2++;
		if (index>=(*this->d_corr_buffer).size()){
			index = 0;
		}
	}
}

//void wcdma_slot_sync_cvc::copy_last_num_samples(gr_complex *buffer,int num_samples)
//{
//	// initialize variables
//	gr_complex sum=0;
//	int index = this->d_corr_buff_pos+1;
//	int index2 = 0;
//
//	if (index>=(*this->d_corr_buffer).size()){
//			index = 0;
//		}
//
//	// copy first position
//	buffer[index2] = (*this->d_corr_buffer)[index];
//	index++;
//	index2++;
//	if (index>=(*this->d_corr_buffer).size()){
//		index = 0;
//	}
//	// loop through and perform the rest of the copying
//	while (index2 < num_samples){
//		buffer[index2] = (*this->d_corr_buffer)[index];
//
//		index++;
//		index2++;
//		if (index>=(*this->d_corr_buffer).size()){
//			index = 0;
//		}
//	}
//}

int wcdma_slot_sync_cvc::find_acc_hit_index()
{
	 float a = 0.0f;
	 int index = -1;

	  for (int i = 0;i< (*this->d_acc_buffer).size();i++){
		  float b = (*this->d_acc_buffer)[i];
		  if (a < b){
			  a=b;
			  index = i;
		  }
	  }
	  return index;
}



/*
 * State functions
 */
void wcdma_slot_sync_cvc::setState(int state, void *param)
{
	in_sync_state_data *in_sync_data = (in_sync_state_data*) this->d_state_data;
	find_frame_sync_state_data *tmp = (find_frame_sync_state_data*) this->d_state_data;

	unsigned char tmp2 = tmp->scrambling_group;

	// clear state data
	for (int i = 0;i<STATE_DATA_SIZE;i++){
		this->d_state_data[i] = 0;
	}

	switch (state){
		case STATE_FIND_FRAME_SYNC:

		case STATE_IN_SYNC:	// need to fix this
			//tmp = *((unsigned char*)param);
//			std::cout << param<<std::endl;
//			std::cout << (void*)&(in_sync_data->scrambling_group)<<std::endl;
//			std::cout << (void*)&(tmp->scrambling_group)<<std::endl;
			in_sync_data->scrambling_group = tmp2;
			in_sync_data->detected_psc_code = -1;
			in_sync_data->PCCPCH_frame_buffer_counter = 0;
			in_sync_data->PCCPCH_frame_buffer = this->d_PCCPCH_frame_buffer;
			in_sync_data->done = &this->d_done;
		default:;
	}
	this->d_state = state;
}

/*
 * waits for some time only to let the radio become stable
 */
int wcdma_slot_sync_cvc::state_wait_on_radio(int noutput_items,
		  gr_vector_int &ninput_items,
		  gr_vector_const_void_star &input_items,
		  gr_vector_void_star &output_items)
{
	int *sdata = (int *)this->d_state_data;

	const gr_complex *in = (const gr_complex *) input_items[0];
	gr_complex *out = (gr_complex *) output_items[0];


	for (int i = 0; i < noutput_items; i++){
		//out[i] = 0.0f;
		sdata[0]++;
	}

	// wait for WAIT_ON_RADIO_NOF_SAMPLES samples
	// then switch state
	if (sdata[0] > WAIT_ON_RADIO_NOF_SAMPLES){
		this->setState(STATE_FIND_SLOT_SYNC,NULL);
	}
	// Tell runtime system how many output items we produced.
	this->consume_each (noutput_items);
	return 0;// noutput_items;
}



/*
 * finds slot synchronization
 */
int wcdma_slot_sync_cvc::state_find_slot_sync(int noutput_items,
		  gr_vector_int &ninput_items,
		  gr_vector_const_void_star &input_items,
		  gr_vector_void_star &output_items)
{
	  const gr_complex *in = (const gr_complex *) input_items[0];
	  gr_complex *out = (gr_complex *) output_items[0];

	  int generated_output_samples = 0;

	  for (int i = 0; i < noutput_items; i++){

		  // place input in buffer
		  (*this->d_corr_buffer).at(this->d_corr_buff_pos) = in[i];

		  // perform correlation
		  float corr_result = this->correlate(0);

		  // put correlation in accumulation buffer
		  float value = (*this->d_acc_buffer).at(this->d_acc_buff_pos);
		  //float alpha = 0.75f;
		  (*this->d_acc_buffer).at(this->d_acc_buff_pos) = value + corr_result;//value*(alpha) + corr_result*(1-alpha);

		  out[generated_output_samples] = corr_result;
		  generated_output_samples++;

	//	  gr_complex output = (*this->d_acc_buffer)[this->d_acc_buff_pos];

		  // produce output (output 1 if we are in index sync with slot else zero)
//		  if (this->d_acc_buff_hit_index >= 0){
//			  if (this->d_acc_buff_hit_index==this->d_acc_buff_pos){
//	//			  tmp = 1.0f;
//				  out[i] = 0.0f;
//				  //std::cout<<".";
//		 	  } else {
//		 		  out[i] = 0.0f;
//		 	  }
//		  } else {
//			   out[i] = 0.0f;
//		  }

	//	  out[i] = tmp;
	//	  tmp -= 0.01f;
	//	  if (tmp<0.0f) tmp=0.0f;

		  // produce output (from accumulation buffer
//		  out[i] = (*this->d_acc_buffer)[this->d_acc_buff_pos];

		  //out[i] = corr_result;
	//	  gr_complex a = (*this->d_acc_buffer)[this->d_acc_buff_pos];
	//	  float alen = sqrt(a.real()*a.real() + a.imag()*a.imag());
	//	  out[i] = gr_complex(alen,0.0f);

		  // increase accumulation buffer index
		  d_acc_buff_pos++;
		  if (d_acc_buff_pos >= (*this->d_acc_buffer).size()){
			  this->d_acc_buff_pos = 0;		// clear d_acc_buff_pos
			  this->d_acc_buff_wrap_cntr++;	// increase wrap counter
			  if (this->d_acc_buff_wrap_cntr > WRAP_COUNTER_RESET){	// if wrap counter should be reset
				  // reset wrap counter
				  // calculate hit in acc buffer
				  // and clear d_acc_buffer
				  this->d_acc_buff_wrap_cntr = 0;
				  int index = this->find_acc_hit_index();
				  this->d_acc_buff_hit_index = index;		// debug 'magic' number




				  if (this->d_acc_buff_hit_index >= (*this->d_acc_buffer).size()){
					  this->d_acc_buff_hit_index -= (*this->d_acc_buffer).size();
				  }




				  std::cout << "I=" << index << ", V=" << (*this->d_acc_buffer)[index] << std::endl;
				  fflush(stdout);

				  int acc_buff_size = (*this->d_acc_buffer).size();
				  for (int i = 0; i< acc_buff_size;i++){
					  (*this->d_acc_buffer)[i] = 0.0f;
				  }



			  }
		  }


		  // move d_corr_buff_pos
		  this->d_corr_buff_pos++;
		  if (this->d_corr_buff_pos>=(*this->d_corr_buffer).size()){
			  this->d_corr_buff_pos=0;
		  }

	  }

	  // toggle state when processing has been completed
	  if (this->d_acc_buff_hit_index >= 0)
	  {
		  this->setState(STATE_FIND_FRAME_SYNC,NULL);
	  }


	  // Tell runtime system how many output items we produced.
	  this->consume_each (noutput_items);
	  return generated_output_samples;//noutput_items;

}



/*
 * find frame synchronization
 */

int wcdma_slot_sync_cvc::find_slot_match(find_frame_sync_state_data *data,int internal_slot_number)
{
	 float a = 0.0f;
	 int index = -1;

	  for (int i = 0;i< 16;i++){
		  float b = data->accumulation[internal_slot_number][i];
		  if (a < b){
			  a=b;
			  index = i;
		  }
	  }
	  return index;
}

static int find_max_index(float *buff, int numItems){
	float a = -10000.0f;
	int index = -1;

	for (int i=0; i<numItems; i++){
		float b = buff[i];
		if (a < b) {
			a=b;
			index = i;
		}
	}

	return index;
}

/*
 * same as wcdma_slot_sync_cvc::check_scrambling_code_groups but only checks the specified group
 */
unsigned short wcdma_slot_sync_cvc::check_scrambling_code_group(unsigned char group_number,unsigned char *codes){

	for (int i = 0;i<15;i++){
		int j2 = 0;
		//if (group_number==12) std::cout << "starting:";
		for (int j = 0;j<15;j++){


			//if (group_number==12) std::cout << ","<<(int)codes[j]<< "?"<<(int)scrambling_code_group_table[group_number][i+j2];
			if (codes[j] != scrambling_code_group_table[group_number][i+j2]){
				break;
			}
			//if (group_number==12) std::cout << "!";
			if (j==14){
				// we have a match
				// i = number of slot offset
				// group_number = the matching group
				unsigned char foffset = 15-i;
				if (foffset==15) foffset = 0;

				return (group_number<<8) + (foffset);
			}
			j2++;
			if (j2+i>=15){
				j2 = -i;
			}
		}
		//if (group_number==12) std::cout << std::endl;
	}
	return 0xFFFF;
}

/*
 * codes should be a pointer to an array of 15 unsigned chars defining the ssc for the last 15 slots
 * returns the scrambling group in the
 * high 8 bits and the slot frame offset in the lower 8 bits
 * return 0xFFFF on error
 */
unsigned short wcdma_slot_sync_cvc::check_scrambling_code_groups(unsigned char *codes)
{
	for (int i = 0;i<64;i++){
		// checking group i
		unsigned short res = this->check_scrambling_code_group(i,codes);
		if (res != 0xFFFF){
			return res;
		}
	}
	return 0xFFFF;
}

int wcdma_slot_sync_cvc::state_find_frame_sync(int noutput_items,
		  gr_vector_int &ninput_items,
		  gr_vector_const_void_star &input_items,
		  gr_vector_void_star &output_items)
{

	const gr_complex *in = (const gr_complex *) input_items[0];
	gr_complex *out = (gr_complex *) output_items[0];

	find_frame_sync_state_data *data = (find_frame_sync_state_data *) this->d_state_data;


	for (int i = 0; i < noutput_items; i++){

		// place input in buffer
		(*this->d_corr_buffer)[this->d_corr_buff_pos] = in[i];

		// loop for data->loop_for_num_samples then go into STATE_IN_SYNC
		if (data->do_loop == true){
			data->loop_for_num_samples--;
			if (data->loop_for_num_samples==0){
				this->d_acc_buff_hit_index=-1;
				this->d_acc_buff_pos=0;
				this->d_corr_buff_pos=0;
				volatile unsigned char sgroup = data->scrambling_group;
				this->setState(STATE_IN_SYNC, (void *)&sgroup);
				this->consume_each (i);
				return 0;//i;
			}
			continue;	// do nothing more this iteration
		}

		// CALCULATE SSC
		if (data->samples_elapsed<17){ // (1-16)
			// correlate ssc(1-16) store result in match
			float corr_result = this->correlate(&data->ssc[0],data->samples_elapsed);
			data->match[data->samples_elapsed-1] = corr_result;

		}
		if (data->samples_elapsed==17){
			// find highest in match
			float a = 0.0f;
			int index = -1;
			for (int j=0;j<16;j++){
				float b = data->match[j];
				if (a < b){
					a=b;
					index = j;
				}
			}

//			// normalize match
//			float max = data->match[index];
//			for (int i=0;i<16;i++){
//				data->match[i] = data->match[i] / max;
//			}

			// add match to accumulation
			for (int i=0;i<16;i++){
				data->accumulation[data->internal_slot_number][i] += data->match[i];
			}

			data->times++;	// count times
		}

		if (data->times > 7*15){
			//calculate and print result
			std::cout << "ssc: ";
			for (int j=0;j<15;j++){
				int sindex = this->find_slot_match(data,j);
				data->ssc_codes[j] = sindex+1;
				if (sindex+1<10){
					std::cout << " " << (sindex+1) /*<<"("<<data->accumulation[i][sindex]*/<<", " ;
				} else {
					std::cout <<  (sindex+1) /*<<"("<<data->accumulation[i][sindex]*/<<", " ;
				}
			}

			std::cout << std::endl;
			std::cout << "Hit index: " << this->d_acc_buff_hit_index << std::endl;

			for (int i2=0;i2<15;i2++){
				for (int j2=0;j2<16;j2++){
					data->accumulation[i2][j2] = 0.0f;
				}
			}
			data->times = 0;
			data->frame_sync_read=true;

			// check the scrambling_code_group_table
			unsigned short framesync_result = this->check_scrambling_code_groups(data->ssc_codes);

			if (framesync_result != 0xFFFF){
				// we found a match
				std::cout << "Scrambling code group: " << ((framesync_result>>8)&0xFF) << std::endl;
				std::cout << "Frame offset: " << (framesync_result&0xFF) << std::endl;

				data->scrambling_group = ((framesync_result>>8)&0xFF);

				// now we need to loop through the frame offset and then loop
				// until this->d_acc_buff_hit_index, then we are in sync

				/*if (this->d_acc_buff_pos>d_acc_buff_hit_index){
					samples_until_slot_sync = this->d_acc_buff_hit_index+(SAMPLES_PER_SLOT-this->d_acc_buff_pos);
				} else {
					samples_until_slot_sync = this->d_acc_buff_hit_index-this->d_acc_buff_pos;
				}*/
				int samples_until_slot_sync = -(512+17);	// hit_index is at +512 samples at each slot + 17 samples for the ssc calculations
				//samples_until_slot_sync = 0;
				// set number of samples to loop
				data->loop_for_num_samples = (SAMPLES_PER_SLOT)*(framesync_result&0xFF)+samples_until_slot_sync;
				if (data->loop_for_num_samples<0){	// we missed the beginning wait for almost one frame then
					data->loop_for_num_samples+=SAMPLES_PER_FRAME;
				}
				std::cout<<"Loop for num samples: "<<data->loop_for_num_samples<<std::endl;

				if (data->loop_for_num_samples == 0) {
					volatile unsigned char sgroup = data->scrambling_group;
					this->setState(STATE_IN_SYNC, (void *)&sgroup);
					return i;
				}

				data->do_loop = true;
				data->frame_sync_read = false;	// so we don't retry finding frame sync
			} else {
				std::cout << "Unable to find frame sync.\n Retrying....." << std::endl;
				this->d_number_of_retries++;
			}
		}

		// produce output (output 1 if we are in index sync with slot else zero)
		if (this->d_acc_buff_hit_index >= 0){
			if (this->d_acc_buff_hit_index==this->d_acc_buff_pos){

				copy_last_512_samples(&data->ssc[0]);

				data->internal_slot_number++;
				if (data->internal_slot_number > 14){
					data->internal_slot_number = 0;
				}
				data->samples_elapsed = 0;
			}
		}

		// produce output (from accumulation buffer
//		out[i] = 1.0f;//(*this->d_acc_buffer)[this->d_acc_buff_pos];

		// increase accumulation buffer index
		d_acc_buff_pos++;
		if (d_acc_buff_pos >= (*this->d_acc_buffer).size()){
			this->d_acc_buff_pos = 0;		// clear d_acc_buff_pos
		}

		// move d_corr_buff_pos
		this->d_corr_buff_pos++;
		if (this->d_corr_buff_pos>=(*this->d_corr_buffer).size()){
			this->d_corr_buff_pos=0;
		}

		data->samples_elapsed++;
	}

	if (data->frame_sync_read == true){
		this->d_acc_buff_hit_index = -1;
		this->setState(STATE_FIND_SLOT_SYNC,NULL);
	}
	// Tell runtime system how many output items we produced.
	this->consume_each (noutput_items);
	return 0;//noutput_items;
}

/**
 * contributes to the calculation of psc_sum and calculates and corrects the slot sync
 */
static void state_in_sync_calculate_psc_sum(in_sync_state_data *data,gr_complex in)
{
	static bool skip_next;

	int ssi = data->frame_sample_index;
	if (ssi == SAMPLES_PER_FRAME-1) ssi = -1;
	if (ssi == SAMPLES_PER_FRAME-2) ssi = -2;
	if (ssi == SAMPLES_PER_FRAME-3) ssi = -3;
	if (ssi == SAMPLES_PER_FRAME-4) ssi = -4;
	if (ssi == SAMPLES_PER_FRAME-5) ssi = -5;

	if ((ssi>=0-5) && (ssi<512-5)){
		data->psc_sum[0] += in * sync_codes[0][ssi+5];
	}
	if ((ssi>=0-4) && (ssi<512-4)){
		data->psc_sum[1] += in * sync_codes[0][ssi+4];
	}
	if ((ssi>=0-3) && (ssi<512-3)){
		data->psc_sum[2] += in * sync_codes[0][ssi+3];
	}
	if ((ssi>=0-2) && (ssi<512-2)){
		data->psc_sum[3] += in * sync_codes[0][ssi+2];
	}
	if ((ssi>=0-1) && (ssi<512-1)){
		data->psc_sum[4] += in * sync_codes[0][ssi+1];
	}

	if ((data->frame_sample_index>=0) && (data->frame_sample_index<512)){
		data->psc_sum[5] += in * sync_codes[0][data->frame_sample_index];
	}

	if ((data->frame_sample_index>=0+1) && (data->frame_sample_index<512+1)){
		data->psc_sum[6] += in * sync_codes[0][data->frame_sample_index-1];
	}
	if ((data->frame_sample_index>=0+2) && (data->frame_sample_index<512+2)){
		data->psc_sum[7] += in * sync_codes[0][data->frame_sample_index-2];
	}
	if ((data->frame_sample_index>=0+3) && (data->frame_sample_index<512+3)){
		data->psc_sum[8] += in * sync_codes[0][data->frame_sample_index-3];
	}
	if ((data->frame_sample_index>=0+4) && (data->frame_sample_index<512+4)){
		data->psc_sum[9] += in * sync_codes[0][data->frame_sample_index-4];
	}
	if ((data->frame_sample_index>=0+5) && (data->frame_sample_index<512+5)){
		data->psc_sum[10] += in * sync_codes[0][data->frame_sample_index-5];
	}


	if (data->frame_sample_index == 600){
		if (skip_next==true){
			skip_next=false;
		} else {
			// calculate correlation value and add it to accumulation
			for (int j=0;j<11;j++){
				gr_complex a = data->psc_sum[j];
				data->psc_accumulation[j] += a.real()*a.real() + a.imag()*a.imag();
				data->psc_sum[j] = 0;
			}

			if (data->frame_cntr%100==0){
				std::cout<<"!";
			}
			if (data->frame_cntr%10*2==0){
				if (data->psc_accumulation[0] > 0.0f && data->psc_accumulation[5] > 0.0f){
					int index = find_max_index(&data->psc_accumulation[0],11);
					//							for (int tmp=0;tmp<11;tmp++){
					//								std::cout<< "["<<tmp<<"]: "<<data->psc_accumulation[tmp]<<std::endl;
					//							}
					int outOfSync = index-6;
					//							if (outOfSync > 1 || outOfSync < -1){
					//								std::cout<<outOfSync;
					//							}


					if (outOfSync != 0){
						data->frame_sample_index -= outOfSync;
						skip_next = true;
						//							std::cout<<"Out of sync="<<outOfSync <<"! Correcting!"<<std::endl;
						std::cout<<outOfSync;//".";
					}


					// clear psc_accumulation data
					for (int j=0;j<11;j++){
						data->psc_accumulation[j] = 0.0f;
					}
				}
			}
		}
	}
}


/*
 * calculates a new z to be used in the descrambling of the recieved samples
 * should be called from the first sample and onwards for every frame since the sequence is calculated online
 */
static gr_complex getZcomplexConjugate(in_sync_state_data *data,int code_number)
{
	// debug stuff
	//return stest[data->frame_sample_index/2];

	if (data->frame_sample_index==0){
		// copy initial sequence
		for (int i=0;i<SCRAMBLING_SEQ_BUFF_LEN;i++){
			for (int j=0;j<4;j++){
				data->xy_scrambling_buffer[i][j] = scrambling_codes[data->scrambling_group*8+code_number][i][j];
			}
		}
	}//memcpy((void*)&scrambling_codes[data->scrambling_group][0][0],&data->xy_scrambling_buffer[0][0],SCRAMBLING_SEQ_BUFF_LEN*4);



	int index = data->frame_sample_index%SCRAMBLING_SEQ_BUFF_LEN;//+2;

	// calculate next value in next sequence
	for (int i=0;i<2;i++){
		data->xy_scrambling_buffer[index+SCRAMBLING_SEQ_BUFF_LEN][i*2] =
				(data->xy_scrambling_buffer[index+14][i*2]+
				data->xy_scrambling_buffer[index][i*2])%2;

		data->xy_scrambling_buffer[index+SCRAMBLING_SEQ_BUFF_LEN][1+i*2] =
				(data->xy_scrambling_buffer[index+20][1+i*2]+
				 data->xy_scrambling_buffer[index+14][1+i*2]+
				 data->xy_scrambling_buffer[index+10][1+i*2]+
				 data->xy_scrambling_buffer[index][1+i*2])%2;
	}

	int z1 = (data->xy_scrambling_buffer[index][0]+data->xy_scrambling_buffer[index][1])%2;
	int z2 = (data->xy_scrambling_buffer[index][2]+data->xy_scrambling_buffer[index][3])%2;
	float re = (z1==1)?-1:1;
	float im = (z2==1)?-1:1;

	// copy newly calculated value downto low part
	data->xy_scrambling_buffer[index][0] = data->xy_scrambling_buffer[index+SCRAMBLING_SEQ_BUFF_LEN][0];
	data->xy_scrambling_buffer[index][1] = data->xy_scrambling_buffer[index+SCRAMBLING_SEQ_BUFF_LEN][1];
	data->xy_scrambling_buffer[index][2] = data->xy_scrambling_buffer[index+SCRAMBLING_SEQ_BUFF_LEN][2];
	data->xy_scrambling_buffer[index][3] = data->xy_scrambling_buffer[index+SCRAMBLING_SEQ_BUFF_LEN][3];


//	if (data->frame_sample_index%SCRAMBLING_SEQ_BUFF_LEN==SCRAMBLING_SEQ_BUFF_LEN-1){
//		// copy next data into current
//		memcpy(&data->xy_scrambling_buffer[SCRAMBLING_SEQ_BUFF_LEN][0],&data->xy_scrambling_buffer[0][0],SCRAMBLING_SEQ_BUFF_LEN*4);
//	}

	return gr_complex(re,-im);
}



/**
 * contributes to the calculation of psc_sum and calculates and corrects the slot sync
 */
static gr_complex decode_p_cpich(in_sync_state_data *data,gr_complex in)
{
	static gr_complex acc;
	static gr_complex old;

	static gr_complex tmp;
	static int code_number;
	static int detected_code = 0;

	gr_complex z;
	if (data->frame_cntr<8*10){
		z = getZcomplexConjugate(data,code_number);
	} else {
		z = getZcomplexConjugate(data, 7);
	}
//	int codenumber = data->frame_sample_index % 4;
//	if (codenumber/2==0){
//		z = gr_complex(1.0f, -1.0f);
//	} else {
//		z = gr_complex(-1.0f,1.0f);
//	}

//	if (data->frame_cntr%100==0 && data->frame_sample_index<36*2){
//		if (data->frame_sample_index==36){
//			std::cout << std::endl;
//		}
//		if (data->frame_sample_index%2==0)
//			std::cout << z <<", ";
//		if (data->frame_sample_index==36-1){
//				std::cout<<std::endl;
//			}
//	}

//	// simple test
//	static gr_complex tmpacc;
//	if (data->frame_sample_index==0){
//		tmpacc = 0.0f;
//	}
//	tmpacc += in*z;
//
//	if (data->frame_sample_index==SAMPLES_PER_FRAME-1){
//
//		float v = tmpacc.real()*tmpacc.real()+tmpacc.imag()*tmpacc.imag();
//		std::cout << "MATCH VALUE: " << v << std::endl;
//		tmpacc = 0.0f;
//
//
//	}
//	//

//	if (code_number!=7){
//		z=0.0f;
//	}
	//z = gr_complex(1,1);


	gr_complex descrambled = in*z;

	if (data->frame_sample_index%512 == 0){
		acc = gr_complex(0.0f,0.0f);
	}
	if (data->frame_sample_index == 0){
		tmp = gr_complex(0.0f,0.0f);
	}

	acc += descrambled;
	tmp += descrambled;

	if (data->frame_sample_index%512==511){
		old = acc;
	}

	if (data->frame_sample_index == SAMPLES_PER_FRAME-1){
		float res = tmp.real()*tmp.real()+tmp.imag()*tmp.imag();
		if (data->frame_cntr<8*10){
			std::cout << "RESULT "<< code_number <<": " << res << std::endl;
		}
		code_number++;
		if (code_number>7) {
			code_number = 0;
		}
	}

	return old;
}


/**
 *
 */
static void detect_psc_code(in_sync_state_data *data, gr_complex in){
	static gr_complex acc;
	static gr_complex old;

	static gr_complex tmp;
	static int code_number;
	static int detected_code = 0;
	static float acc_res[8] = {0,0,0,0,0,0,0,0};
	gr_complex z;

	if (data->frame_cntr<8*10){
		z = getZcomplexConjugate(data,code_number);
	} else {
		return;
	}

	gr_complex descrambled = in*z;

	if (data->frame_sample_index%512 == 0){
		acc = gr_complex(0.0f,0.0f);
	}
	if (data->frame_sample_index == 0){
		tmp = gr_complex(0.0f,0.0f);
	}

	acc += descrambled;
	tmp += descrambled;

	if (data->frame_sample_index%512==511){
		old = acc;
	}

	if (data->frame_sample_index == SAMPLES_PER_FRAME-1){
		float res = tmp.real()*tmp.real()+tmp.imag()*tmp.imag();
//		if (data->frame_cntr<8*10){
//			std::cout << "RESULT "<< code_number <<": " << res << std::endl;
//		}

		acc_res[code_number] += res;
		code_number++;
		if (code_number>7) {
			code_number = 0;
		}
	}

	if (data->frame_cntr==(8*10-1)){

			float max = -1;
			for (int i=0;i<8;i++){
				std::cout << i << ": " << acc_res[i] << std::endl;
				if (max < acc_res[i]){
					data->detected_psc_code = i;
					max = acc_res[i];
				}
			}
		std::cout << "Detected SC nr: " << data->detected_psc_code << std::endl;
	}
}


static void cpich_decode(in_sync_state_data *data)
{
	static gr_complex acc;
	static gr_complex old;

	static int code_number;

	if (data->frame_sample_index%512 == 0){
		acc = gr_complex(0.0f,0.0f);
	}

	acc += data->last_descrambled_chip;// * gr_complex(1.0f,1.0f); // * gr_complex(1.0f,-1.0f);

	if (data->frame_sample_index % 512 == 511){
		float alpha = 1.0f;

		data->last_CPICH_symbol = acc * alpha + data->last_CPICH_symbol * (1-alpha);
		// simplified cross product in order to find directiong of rotation from 1+1j
		// 0 = online(no rotation), + = counter clockwise, - = clockwise
		float dir = data->last_CPICH_symbol.real() - data->last_CPICH_symbol.imag();

		float l1 = sqrt(data->last_CPICH_symbol.real()*data->last_CPICH_symbol.real() + data->last_CPICH_symbol.imag()*data->last_CPICH_symbol.imag());
		float l2 = 1.414213562f; // sqrt(2);

		// simplified dot product against 1+1j
		float dot = data->last_CPICH_symbol.real() + data->last_CPICH_symbol.imag();

		if (dir > 0) {dir = 1;} else {dir = -1;}

		float l3 = l1*l2;

		static float lookup = 1.0f;

		if (l3 != 0 && !isnan(l3)) {
			lookup = ((dot /l1) / l2);
		}

		float angle = acos(lookup)*dir;
		//angle-=3.1415/4;
		//if (data->frame_cntr%512==0) {std::cout << "Angle: " << angle << " dot: " << dot<< " lookup: " << lookup;}
		float m[4];
		m[0] = cos(angle);
		m[1] = -sin(angle);
		m[2] = sin(angle);
		m[3] = cos(angle);

		gr_complex a = gr_complex(
				m[0] * data->last_CPICH_symbol.real() + m[1] * data->last_CPICH_symbol.imag(),
				m[2] * data->last_CPICH_symbol.real() + m[3] * data->last_CPICH_symbol.imag()
				);

		data->last_rotated_CPICH_symbol = a;
		//data->last_rotated_CPICH_symbol = data->last_CPICH_symbol;
//		std::cout << data->last_rotated_CPICH_symbol.real() << ", "
//				<< data->last_rotated_CPICH_symbol.imag() << ". ";
		// copy phase rotation matrix
		data->CPICHRotMatrix[0] = m[0];
		data->CPICHRotMatrix[1] = m[1];
		data->CPICHRotMatrix[2] = m[2];
		data->CPICHRotMatrix[3] = m[3];

	}
}

static void pccpch_decode(in_sync_state_data *data)
{
	// perform decoding of the pccpch
	static gr_complex acc;
		static gr_complex old;

		static int code_number;
		static int symbol_index = 0;

		// no data on PCCPCH on the first 512 half chips
		if (data->frame_sample_index%5120 < 512){
			return;
		}

		int part = (data->frame_sample_index%512);

		if (part == 0){
			acc = gr_complex(0.0f,0.0f);

		}

		// apply channelisation code 256,1
		if (part<256){
			acc += data->last_descrambled_chip;
		}
		else
		{
			gr_complex tmp = gr_complex(data->last_descrambled_chip*-1.0f);
			acc += tmp;
		}

		// entire symbol decoded phase rotate and save
		if (part==511){
			data->last_PCCPCH_symbol = acc;

			data->last_rotated_PCCPCH_symbol = gr_complex(
					data->CPICHRotMatrix[0] * data->last_PCCPCH_symbol.real() + data->CPICHRotMatrix[1] * data->last_PCCPCH_symbol.imag(),
					data->CPICHRotMatrix[2] * data->last_PCCPCH_symbol.real() + data->CPICHRotMatrix[3] * data->last_PCCPCH_symbol.imag()
					);

			//data->last_rotated_PCCPCH_symbol = data->last_PCCPCH_symbol;

			int b1,b2;

			b1 = data->last_rotated_PCCPCH_symbol.real() < 0.0f;
		    b2 = data->last_rotated_PCCPCH_symbol.imag() < 0.0f;

// Should be as this
//		    b1 = data->last_rotated_PCCPCH_symbol.real() < 0.0f;
//		    b2 = data->last_rotated_PCCPCH_symbol.imag() < 0.0f;

			data->PCCPCH_last_frame[symbol_index*2] = b1;
			data->PCCPCH_last_frame[symbol_index*2+1] = b2;

			if (data->PCCPCH_frame_buffer_counter < PCCPCH_FRAME_BUFFER_SIZE){
				// write data
				(*data->PCCPCH_frame_buffer)[data->PCCPCH_frame_buffer_counter*270+symbol_index*2] = b1;
				(*data->PCCPCH_frame_buffer)[data->PCCPCH_frame_buffer_counter*270+symbol_index*2+1] = b2;
			} else {
				//std::cout << "PCCPCH_frame_buffer_counter: " << data->PCCPCH_frame_buffer_counter << std::endl;
				*(data->done) = true;
			}

			symbol_index++;
			if (symbol_index>=135){
				// increase index
				if (data->PCCPCH_frame_buffer_counter<PCCPCH_FRAME_BUFFER_SIZE)
				{
				  data->PCCPCH_frame_buffer_counter++;
				}
				std::cout << "\nPCCPCH data: ";
				for (int i = 0;i<270;i++){
					std::cout << (int) data->PCCPCH_last_frame[i];
				}
				std::cout << std::endl;

				symbol_index = 0;
			}

		}
		//half chip in slot
		//data->frame_sample_index%5120;


}



/*
 * The block is in this state when frame sync has been achieved
 */
int wcdma_slot_sync_cvc::state_in_sync(int noutput_items,
		  gr_vector_int &ninput_items,
		  gr_vector_const_void_star &input_items,
		  gr_vector_void_star &output_items)
{
	int generated_output_samples;
	generated_output_samples = 0;

	const gr_complex *in = (const gr_complex *) input_items[0];
	gr_complex *out = (gr_complex *) output_items[0];

	in_sync_state_data *data = (in_sync_state_data*)this->d_state_data;

	for (int i = 0; i < noutput_items; i++){
//		out[i] = in[i].real()*in[i].real() + in[i].imag()*in[i].imag();
//		if (data->frame_sample_index>=0 && data->frame_sample_index<=15){
//			out[i] = 1000000.0f;
//		}

		// check that we're still in sync
		// by calculating psc correlations into data->psc_accumulation
		// and changeing the data->frame_sample_index accordingly

#ifndef DISABLE_SLOT_SYNC_WHEN_IN_SYNCH
		state_in_sync_calculate_psc_sum(data,in[i]);
#endif

		if (data->frame_cntr<8*10 && data->detected_psc_code < 0){
			detect_psc_code(data, in[i]);  // perform detection of syncronization code
		} else {
			data->last_scrambling_chip_Z = getZcomplexConjugate(data,data->detected_psc_code);
			data->last_chip = in[i];
			data->last_descrambled_chip = data->last_chip * data->last_scrambling_chip_Z;
			cpich_decode(data);
			pccpch_decode(data);
		}

		//gr_complex symbol = decode_p_cpich(data,in[i]);

		if (data->frame_sample_index%512==0){
			out[generated_output_samples] = data->last_CPICH_symbol;//data->last_rotated_CPICH_symbol;//data->last_CPICH_symbol;//data->last_PCCPCH_symbol;//last_rotated_PCCPCH_symbol;//symbol;
			generated_output_samples++;
		}

//		if (data->frame_cntr%500==0){
//			for (int j=1;j<20;j++){
//				if (data->frame_sample_index==512*j){
//					std::cout <<"J=="<<j<<", "<<out[i]<<", ";
//				}
//			}
//		}
//		static gr_complex tmpacc= 0.0f;
//		if (data->frame_sample_index<512){
//			tmpacc += in[i] * sync_codes[0][data->frame_sample_index];
//		}
//		if (data->frame_sample_index==512 && data->frame_cntr%(15*3)){
//			float v = tmpacc.real()*tmpacc.real()+tmpacc.imag()*tmpacc.imag();
//			std::cout << v << ", ";
//			tmpacc = 0.0f;
//		}

		// only print how many frames that passed
//		if (data->frame_cntr%500==499 && data->frame_sample_index==SAMPLES_PER_FRAME-1){
//			//std::cout<<"500 frames passed!"<<((int)data->scrambling_group)<<std::endl;
//		}

//		if (data->frame_cntr%1000==499 && data->frame_sample_index==SAMPLES_PER_FRAME-1){
//			std::cout<<"500 frames passed!"<<((int)data->scrambling_group)<<std::endl;
//			this->setState(STATE_FIND_SLOT_SYNC,NULL);
//			return i;
//		}


		///////////////////
		// increase frame and sample counters
		data->frame_sample_index++;
		if ( data->frame_sample_index >= SAMPLES_PER_FRAME){
			data->frame_sample_index = 0;

			data->frame_cntr++;
			if (data->frame_cntr>=1000000){
				data->frame_cntr = 0;
			}
		}

	}


	// Tell runtime system how many output items we produced.
	this->consume_each (noutput_items);
	return generated_output_samples;
}


void wcdma_slot_sync_cvc::print_buffers()
{

	std::cout << "Correlation buffer: [" ;
	for (int i=0;i<(*this->d_corr_buffer).size();i++){
		std::cout << (*this->d_corr_buffer)[i] << ", ";
	}
	std::cout << "]\n";

	//	  std::cout << "Correlation string: [" ;
	//	  for (int i=0;i<(*this->d_corr_buffer).size();i++){
	//		  std::cout << (*this->d_corr_str)[i] << ", ";
	//	  }

	std::cout << "]\n";
	std::cout << "Current index: " << this->d_corr_buff_pos;

}

unsigned char wcdma_slot_sync_cvc::getPCCPCHData(int frame, int i)
{
	return (*this->d_PCCPCH_frame_buffer)[frame*270+i];
}

int wcdma_slot_sync_cvc::isDone()
{
	return this->d_done;
}

int wcdma_slot_sync_cvc::getNumberOfRetries()
{
	return this->d_number_of_retries;
}

